#include "FreeRTOS.h"
#include "task.h"
#include "nxbot.h"
//#include "sonar.h"



static void gripperTask(void *pvParameters);


void executeGripperTask(globals_t *pvParameters)
{

	xTaskCreate(gripperTask,(const signed portCHAR * const)"gripperTask",gripperTask_stackSize,(void *)pvParameters,tskIDLE_PRIORITY + 2,( xTaskHandle * ) NULL);
}

static void gripperTask(void *pvParameters)
{	

	
	//convertion of parameters 
	globals_t *globals;
	globals=(globals_t *)pvParameters;
	
	//delay
	unsigned int ticks = TICKS_GRIPPER_CLOSE;
	portTickType actualTick;
	
	//conteo de 5 segundos
	unsigned char initCount = OFF;
	portTickType initTicks = xTaskGetTickCount();
	
	
	DDRC |= (1<<PC4);
	
	for(;;)
	{
	
	  //define if gripper is going to be closed or opened
		if(globals->gripperState == GRIPPER_CLOSE)
		{
		  //se verifica si previamente tenia este valor, para hacer el conteo de 5 seg y apagar el gripper (consumo de corriente)
		  if(ticks != TICKS_GRIPPER_CLOSE)
		  {
		    initCount = ON;
		    initTicks = xTaskGetTickCount();			
		  }
		  
		  ticks = TICKS_GRIPPER_CLOSE;
		}
		else
		{
		  //se verifica si previamente tenia este valor, para hacer el conteo de 5 seg y apagar el gripper (consumo de corriente)
		  if(ticks != TICKS_GRIPPER_OPEN)
		  {
		    initCount = ON;
		    initTicks = xTaskGetTickCount();			
		  }
		    		
		  ticks = TICKS_GRIPPER_OPEN;
		}
		  
		//verifica si ya se cumplieron los 5 seg
		
		actualTick = xTaskGetTickCount();		
		
		if(initCount == ON)
		{
		  if(actualTick < initTicks)
		    if(actualTick + (65535 - initTicks) >= TICKS_GRIPPER_WORKS)
		      initCount = OFF;
		  else
	  	  if(actualTick - initTicks >= TICKS_GRIPPER_WORKS)
    		  initCount = OFF;
		}
		  
		
		//si aun no se han cumplido los 5 seg, el gripper se abre o cierra
		if(initCount == ON)
		{  
		  PORTC |= (1<<PC4);
		  actualTick = xTaskGetTickCount();			
		
		  vTaskDelayUntil( &actualTick,(portTickType)ticks);	
		
		
		  PORTC &= ~(1<<PC4);
		  actualTick = xTaskGetTickCount();			
		
		  vTaskDelayUntil( &actualTick,(portTickType)(TICKS_GRIPPER_FREQ-ticks));	
		}
		//si ya se cumplieron los 5 seg, el gripper se apaga.
		else
		  PORTC &= ~(1<<PC4);
		
		


	}
}



